/*
 * filtering_and_tracking1.cpp
 *
 *  Created on: Nov 24, 2022
 *      Author: xidian
 */

#include "include/filtering_and_tracking1.h"
#include <math.h>
#include <assert.h>

#define CIRCLE_RADAR 0
#define KURADAR 0
#define C_RADAR 1
#define printf 0
#define WORK_MODE 0

DataProcessing::DataProcessing() {
    //initialize
#if CIRCLE_RADAR
    dt = 1.5f;
    doppler_max = 1024;
    m_track_id = 0;
    m_sector_id = 0;
    iir_forget_factor = 0.5f;
    m_a_inc = 0.25f;
#endif
#if KURADAR
    m_track_id = 0;
    m_sector_id = 0;
    iir_forget_factor = 0.0f;
    m_a_inc_x = 0.0f;
    m_a_inc_y = 0.0f;
    m_azimuth_threshold_ref = 4.50f * pi / 180.0f;
    m_range_threshold_ref = float(5.0 * 2.80f);
    high_filter_error_coeff = 8.0f;

#endif
//TODO 雷达参数调节
#if C_RADAR
    dt = 3.0;
    doppler_max = 64;
    doppler_res = 64 / doppler_max * 2.3;
    m_track_id = 0;
    m_sector_id = 0;
    iir_forget_factor = 0.0f;
    m_a_inc_x = 0.0f;
    m_a_inc_y = 0.0f;

#endif
    sector_count_front = 0;
    sector_count_back = 0;
    m_MatrixOprt = new MatrixOperations();
    //kalman filter
    memset(m_KalmanFilterParam.F, 0x0, sizeof(float) * 6 * 6);
    memset(m_KalmanFilterParam.Q, 0x0, sizeof(float) * 6 * 6);
    memset(m_KalmanFilterParam.Q1, 0x0, sizeof(float) * 6 * 6);
    memset(m_KalmanFilterParam.H, 0x0, sizeof(float) * 3 * 6); //H是不断更新变化的
    memset(m_KalmanFilterParam.R, 0x0, sizeof(float) * 3 * 3); //R是随着点迹变化的
    memset(m_KalmanFilterParam.B, 0x0, sizeof(float) * 3 * 6);

    m_KalmanFilterParam.F[0 * 6 + 0] = 1;
    m_KalmanFilterParam.F[0 * 6 + 3] = dt;
    m_KalmanFilterParam.F[1 * 6 + 1] = 1;
    m_KalmanFilterParam.F[1 * 6 + 4] = dt;
    m_KalmanFilterParam.F[2 * 6 + 2] = 1;
    m_KalmanFilterParam.F[2 * 6 + 5] = dt;

    m_KalmanFilterParam.F[3 * 6 + 3] = 1;
    m_KalmanFilterParam.F[4 * 6 + 4] = 1;
    m_KalmanFilterParam.F[5 * 6 + 5] = 1;

#if CIRCLE_RADAR
    float delta_w = 1.0e-3f;
    m_KalmanFilterParam.Q[0 * 4 + 0] = delta_w * 0.5f * powf(dt, 2.0);//a
    m_KalmanFilterParam.Q[1 * 4 + 1] = delta_w * 0.5f * powf(dt, 2.0);//a
    m_KalmanFilterParam.Q[2 * 4 + 2] = delta_w * 0.5f * dt;//c
    m_KalmanFilterParam.Q[3 * 4 + 3] = delta_w * 0.5f * dt;//c
    m_KalmanFilterParam.R[0 * 2 + 0] = 100.0f;
    m_KalmanFilterParam.R[1 * 2 + 1] = powf(5.0f * pi / 180.0f, 2.0f);
#endif
#if KURADAR
    float delta_w = 5.0e-2f;
    m_KalmanFilterParam.Q[0 * 4 + 0] = delta_w * 0.33f * powf(dt, 3.0);//a
    m_KalmanFilterParam.Q[1 * 4 + 1] = delta_w * 0.5f * powf(dt, 2.0);//a
    m_KalmanFilterParam.Q[2 * 4 + 2] = delta_w * 0.5f * powf(dt, 2.0);//c
    m_KalmanFilterParam.Q[3 * 4 + 3] = delta_w * dt;//c
    m_KalmanFilterParam.R[0 * 2 + 0] = powf(0.2f * 6.0f, 2.0f);
    m_KalmanFilterParam.R[1 * 2 + 1] = powf(0.3f * pi / 180.0f, 2.0f);
#endif
#if C_RADAR
    float delta_w = 0.5f;
    m_KalmanFilterParam.Q[0 * 6 + 0] = delta_w * pow(dt, 4) / 4;//x x
    m_KalmanFilterParam.Q[0 * 6 + 3] = delta_w * pow(dt, 3) / 2;//x vx
    m_KalmanFilterParam.Q[1 * 6 + 1] = delta_w * pow(dt, 4) / 4;//y y
    m_KalmanFilterParam.Q[1 * 6 + 4] = delta_w * pow(dt, 3) / 2;//y vy
    m_KalmanFilterParam.Q[2 * 6 + 2] = delta_w * pow(dt, 4) / 4;//z z
    m_KalmanFilterParam.Q[2 * 6 + 5] = delta_w * pow(dt, 3) / 2;//z vz
    m_KalmanFilterParam.Q[3 * 6 + 0] = delta_w * pow(dt, 3) / 2;//vx x
    m_KalmanFilterParam.Q[3 * 6 + 3] = delta_w * pow(dt, 2);// vx vx
    m_KalmanFilterParam.Q[4 * 6 + 1] = delta_w * pow(dt, 3) / 2;//vy y
    m_KalmanFilterParam.Q[4 * 6 + 4] = delta_w * pow(dt, 2);// vy vy
    m_KalmanFilterParam.Q[5 * 6 + 2] = delta_w * pow(dt, 3) / 2;//vz z
    m_KalmanFilterParam.Q[5 * 6 + 5] = delta_w * pow(dt, 2);// vz vz
    //误差观测矩阵
    float r = 1;//误差系数
    m_KalmanFilterParam.R[0 * 3 + 0] = r * powf(0.3f * 3.75f, 2.0f);
    m_KalmanFilterParam.R[1 * 3 + 1] = r * powf(0.2f * 8.0f * pi / 180.0f, 2.0f);
    m_KalmanFilterParam.R[2 * 3 + 2] = r * powf(0.2f * 8.0f * pi / 180.0f, 2.0f);

#endif

    m_KalmanFilterParam.B[0 * 2 + 0] = 0.5f * dt * dt;
    m_KalmanFilterParam.B[1 * 2 + 1] = 0.5f * dt * dt;
    m_KalmanFilterParam.B[2 * 2 + 2] = 0.5f * dt * dt;
    m_KalmanFilterParam.B[3 * 2 + 0] = dt;
    m_KalmanFilterParam.B[4 * 2 + 1] = dt;
    m_KalmanFilterParam.B[5 * 2 + 2] = dt;

    m_KalmanFilterParam.F_v[0] = 1.0f;
    m_KalmanFilterParam.F_v[1] = 0.0f;     //dt;
    m_KalmanFilterParam.F_v[2] = 0.0f;
    m_KalmanFilterParam.F_v[3] = 1.0f;

    m_KalmanFilterParam.Q_v[0] = 0.5f * dt * dt;
    m_KalmanFilterParam.Q_v[1] = 0.0f;
    m_KalmanFilterParam.Q_v[2] = 0.0f;
    m_KalmanFilterParam.Q_v[3] = dt;

    m_KalmanFilterParam.measurement_noise_var = 1.0f;

    init_map();
    uint32_t id_count = 3000000;
    init_id_cache(id_count);
}

DataProcessing::~DataProcessing() {
    if (m_MatrixOprt != nullptr) {
        delete m_MatrixOprt;
        m_MatrixOprt = nullptr;
    }
}

void DataProcessing::correlate_with_active_tracks(std::vector<PointInfo> &input,
                                                  std::vector<TrackInfo> &track,
                                                  std::vector<associated_struct> &output
) {
    /*ͨ 通过径向增量(有默认值保护) + 距离增量(最大最小) + 多普勒增量 + 角度增量（有默认值保护） 来进行相关*/
    /*ͨ通过多普勒增量和距离增量加权来找到最优点的相关点，放在相关列表第一个*/
    float range_threshold = 0.0f;            //径向改变量门限
    float doppler_threshold = 0.0f;            //多普勒门限
    float azimuth_threshold = 0.0f;            //方位门限

    float phi_old = 0.0f;
    float phi_th = 0.0f;
    float phi_threshold = 0.0f;
    float v_TH = 0.0f;

    float range_error = 0.0f;
    float azimuth_error = 0.0f;
    float doppler_error = 0.0f;
    float phi_error = 0.0f;
    float v_abs = 0.0f;
    float judgment_val = 0.0f;
    float judgment_threshold = 10000;

    associated_struct elem_topush;
    float rang_change = 0.0f;
    float distance_increament = 0.0f;
    float pitch_input = 0.0f;


    for (std::size_t i = 0; i < track.size(); i++) {

        elem_topush.associated_count = 0;
        float r_old = sqrt(powf(track.at(i).filtered[0], 2.0f) + powf(track.at(i).filtered[1], 2.0f) +
                           powf(track.at(i).filtered[2], 2.0f));
        float r_pre = sqrt(powf(track.at(i).predicted[0], 2.0f) + powf(track.at(i).predicted[1], 2.0f) +
                           powf(track.at(i).predicted[2], 2.0f));
        //稳态航迹关联门限值
        range_threshold = 10;
        doppler_threshold = 4;
        azimuth_threshold = 3.0;
        phi_threshold = 3.0;
        judgment_threshold = 10000;

        for (std::size_t j = 0; j < input.size(); j++) {
            if (input.at(j).correState == NONE) {
                //增量
                float range = sqrt(powf(input.at(j).pos_x, 2.0f) + powf(input.at(j).pos_y, 2.0f) +
                                   powf(input.at(j).pos_z, 2.0f));
                if (range > 3000)
                    range_threshold = 16;
                float H_phi = atan2(track.at(i).predicted[2],
                                    sqrt(powf(track.at(i).predicted[1], 2.0f) + powf(track.at(i).predicted[0], 2.0f))) *
                              180 / pi;
                float H_azi = atan2(track.at(i).predicted[1], track.at(i).predicted[0]) / pi * 180;
                range_error = fabs(range - r_pre);
                azimuth_error = fabs(H_azi - input.at(j).azi_angle);
                doppler_error = fabs(track.at(i).doppler_predicted[0] - input.at(j).doppler);
                phi_error = fabs(H_phi - input.at(j).pitch_angle);

                //TODO：稳态航迹关联
                if (range_error < range_threshold
                    && azimuth_error < azimuth_threshold
                    && phi_error < phi_threshold
                    && doppler_error < doppler_threshold) {

                    if (elem_topush.associated_count > this->m_max_associated_count) {
                        std::cout << "超出最大关联点数" << std::endl;
                    } else {
                        elem_topush.track_id = track.at(i).track_seq;
                        elem_topush.associated_count += 1;
                        elem_topush.point_id[elem_topush.associated_count - 1] = j;
                        input.at(j).correState = WITH_ACTIVE_TRACK;
                        judgment_val = 0.1f * range_error + 0.2f * doppler_error + 0.7 * azimuth_error
                                       - input.at(j).SNR / 50;
//                        judgment_val = input.at(j).SNR;
                        if (judgment_val < judgment_threshold) {
                            judgment_threshold = judgment_val;
                            int tmp_point_id = elem_topush.point_id[0];
                            elem_topush.point_id[0] = elem_topush.point_id[elem_topush.associated_count - 1];
                            elem_topush.point_id[elem_topush.associated_count - 1] = tmp_point_id;
                        }
                    }
                }
            }
        }

        //稳态航迹发生机动关联门限值
        range_threshold = 15;
        doppler_threshold = 5;
        v_TH = 20.0;
        azimuth_threshold = 4.0;
        phi_threshold = 3.0;
        judgment_threshold = 10000;
        if (elem_topush.associated_count == 0) {
            for (std::size_t j = 0; j < input.size(); j++) {
                if (input.at(j).correState != WITH_ACTIVE_TRACK) {
                    //增量
                    doppler_error = fabs(track.at(i).doppler_filtered[0] - input.at(j).doppler);
                    azimuth_error = fabs(atan2(track.at(i).filtered[1], track.at(i).filtered[0]) -
                                         atan2(input.at(j).pos_y, input.at(j).pos_x)) / pi * 180;
                    float input_r = sqrt(powf(input.at(j).pos_x, 2.0f) + powf(input.at(j).pos_y, 2.0f) +
                                         powf(input.at(j).pos_z, 2.0f));
                    float track_r = sqrt(powf(track.at(i).filtered[0], 2.0f) +
                                         powf(track.at(i).filtered[1], 2.0f) +
                                         powf(track.at(i).filtered[2], 2.0f));
                    range_error = input_r - track_r;
                    float r_increament = ((track.at(i).doppler_filtered[0] - doppler_max / 2) * 0.5 +
                                          (input.at(j).doppler - doppler_max / 2) * 0.5) * doppler_res * dt;
                    range_error = fabs(range_error - r_increament);
                    distance_increament = sqrt(powf(track.at(i).filtered[0] - input.at(j).pos_x, 2.0f) +
                                               powf(track.at(i).filtered[1] - input.at(j).pos_y, 2.0f) +
                                               powf(track.at(i).filtered[2] - input.at(j).pos_z, 2.0f));
                    v_TH = 20 + track_r*1/180*pi/dt;
//                    v_abs = abs(track_r- input_r)/ dt;
                    v_abs =distance_increament / dt;
                    float F_phi = atan2(track.at(i).filtered[2], sqrt(powf(track.at(i).filtered[1], 2.0f) +
                                                                      powf(track.at(i).filtered[0], 2.0f))) * 180 / pi;
                    phi_error = fabs(F_phi - input.at(j).pitch_angle);

                    //TODO 进行机动相关
                    if (doppler_error < doppler_threshold
                        && range_error < range_threshold
                        && phi_error < phi_threshold
                        && azimuth_error < azimuth_threshold
                        && v_abs < v_TH
                            ) {
                        if (elem_topush.associated_count > this->m_max_associated_count) {
                            std::cout << "超出最大关联点数" << std::endl;
                        } else {
                            elem_topush.track_id = track.at(i).track_seq;
                            elem_topush.associated_count += 1;
                            elem_topush.point_id[elem_topush.associated_count - 1] = j;
                            input.at(j).correState = WITH_ACTIVE_TRACK_JIDONG;
                            judgment_val = range_error - input.at(j).SNR;
                            track.at(i).isManeuverable = true;
                            if (judgment_val < judgment_threshold) {
                                judgment_threshold = judgment_val;
                                int tmp_point_id = elem_topush.point_id[0];
                                elem_topush.point_id[0] = elem_topush.point_id[elem_topush.associated_count - 1];
                                elem_topush.point_id[elem_topush.associated_count - 1] = tmp_point_id;
                            }
                        }

                    }
                }
            }
        }

        if (elem_topush.associated_count > 0) {
            output.push_back(elem_topush);
            track.at(i).isUpdated = true;
        }
    }
}

void DataProcessing::correlate_with_detective_tracks(std::vector<PointInfo> &input,
                                                     std::vector<TrackInfo> &track,
                                                     std::vector<associated_struct> &output) {
    /*关联原则： 径向距离变化 && 距离增量 && 多普勒增量 && 方位增量 && 方向 */
    /*ͨ通过多普勒增量和距离增量加权来找到最优点的相关点，放在相关列表第一个*/
    float range_threshold = 0.0f;                        //径向改变量门限
    float doppler_threshold = 0.0f;                        //多普勒门限
    float phi_threshold = 0.0f;                            //俯仰门限
    float azimuth_threshold = 0.0f;                        //方位门限


    float doppler_increament = 0.0f;
    float azimuth_increament = 0.0f;
    float phi_increament = 0.0f;


    associated_struct elem_topush;
    std::vector<associated_struct> list;
    std::vector<TrackInfo> vector_track;


    for (std::size_t i = 0; i < track.size(); i++) {

        elem_topush.associated_count = 0;
        float r_old = sqrt(powf(track.at(i).filtered[0], 2.0f) + powf(track.at(i).filtered[1], 2.0f) +
                           powf(track.at(i).filtered[2], 2.0f));
        float r_pre = sqrt(powf(track.at(i).predicted[0], 2.0f) + powf(track.at(i).predicted[1], 2.0f) +
                           powf(track.at(i).predicted[2], 2.0f));
        float H_phi = atan2(track.at(i).predicted[2],
                            sqrt(powf(track.at(i).predicted[1], 2.0f) + powf(track.at(i).predicted[0], 2.0f))) * 180 /
                      pi;
        //暂态航迹关联门限值
        range_threshold = 8;
        doppler_threshold = 3;
        azimuth_threshold = 2.0f;
        phi_threshold = 2.0f;
        if (r_old < 500) {
            phi_threshold = 5.0f;
            azimuth_threshold = 3.0f;
        }


        for (std::size_t j = 0; j < input.size(); j++) {
            if (input.at(j).correState == NONE)//点迹没有被成功关联过
            {
                //增量
                doppler_increament = fabs(track.at(i).doppler_predicted[0] - input.at(j).doppler);
                azimuth_increament = fabs(atan2(track.at(i).predicted[1], track.at(i).predicted[0]) * 180 / pi -
                                          input.at(j).azi_angle);
                phi_increament = fabs(H_phi - input.at(j).pitch_angle);
                float range = sqrt(powf(input.at(j).pos_x, 2.0f) + powf(input.at(j).pos_y, 2.0f)
                                   + powf(input.at(j).pos_z, 2.0f));
                float r_error = fabs(range - r_pre);

                //TODO:暂态航迹关联
                if (doppler_increament < doppler_threshold
                    && r_error < range_threshold
                    && phi_increament < phi_threshold
                    && azimuth_increament < azimuth_threshold
                        ) {
                    if (elem_topush.associated_count > this->m_max_associated_count) {
                        std::cout << "超出最大关联点数" << std::endl;
                    } else {
                        elem_topush.track_id = track.at(i).track_seq;
                        elem_topush.associated_count += 1;
                        elem_topush.point_id[elem_topush.associated_count - 1] = j;
                        input.at(j).correState = WITH_DETECTIVE_TRACK;
                    }
                }
            }
        }
        if (elem_topush.associated_count > 0) {
            list.push_back(elem_topush);
            track.at(i).isUpdated = true;
        }
    }

    for (std::size_t i = 0; i < list.size(); i++) {
        if (list.at(i).associated_count > 1)//关联多条航迹
        {
            for (std::size_t j = 0; j < track.size(); j++) {
                if (track.at(j).track_seq == list.at(i).track_id) {
                    elem_topush.point_id[0] = list.at(i).point_id[0];
                    elem_topush.track_id = track.at(j).track_seq;
                    elem_topush.associated_count = 1;
                    output.push_back(elem_topush);
                    for (uint32_t k = 1; k < list.at(i).associated_count; k++)//重新生成暂定航迹
                    {
                        vector_track.push_back(track.at(j));
                        //start_count暂时为负值，在状态更新时用来区分是否为分叉出的航迹，并重新给值
                        vector_track.at(vector_track.size() - 1).start_count = -1;
                        vector_track.at(vector_track.size() - 1).deletion_threshold = 0;
                        vector_track.at(vector_track.size() - 1).track_seq = m_id_cache.front();
                        m_id_cache.pop();
                        elem_topush.point_id[0] = list.at(i).point_id[k];
                        elem_topush.track_id = vector_track.at(vector_track.size() - 1).track_seq;
                        elem_topush.associated_count = 1;
                        output.push_back(elem_topush);
                    }
                }
            }
        } else {
            elem_topush.point_id[0] = list.at(i).point_id[0];
            elem_topush.track_id = list.at(i).track_id;
            elem_topush.associated_count = 1;
            output.push_back(elem_topush);
        }
    }

    for (std::size_t i = 0; i < vector_track.size(); i++)
        track.push_back(vector_track.at(i));
}

void DataProcessing::correlate_with_points(std::vector<PointInfo> &input,
                                           std::vector<PointTemp> &points,
                                           std::vector<associated_struct> &output) {
    /*关联原则： 速度 && 距离 && 加速度 && 方位 && 方向 */
    float range_threshold = 0.0f;               //径向距离门限
    float azimuth_threshold = 0.0f;             //方位门限
    float phi_threshold = 0.0f;

    float doppler_threshold_max = 0.0f;        //多普勒门限 上限
    float v_TH = 0.0f;

    float distance_increament = 0.0f;
    float range_increament = 0.0f;
    float azimuth_increament = 0.0f;
    float phi_increament = 0.0f;
    float v_abs = 0.0f;
    int doppler_change = 0;

    associated_struct elem_topush;


    for (std::size_t i = 0; i < points.size(); i++) {
        elem_topush.associated_count = 0;
        elem_topush.track_id = points.at(i).point_seq;

        //暂态点迹关联门限
        range_threshold = 6;
        azimuth_threshold = 1.0f;
        doppler_threshold_max = 2;
        phi_threshold = 1.5f;

        if (points.at(i).r < 400) {
            phi_threshold = 5.0f;
        }
        v_TH = 20;
        float point_r = sqrt(powf(points.at(i).pos_x, 2.0f) + powf(points.at(i).pos_y, 2.0f) +
                             powf(points.at(i).pos_z, 2.0f));
        if (point_r > 3000) {
            azimuth_threshold = 1.0f;
            range_threshold = 10.f;
            phi_threshold = 1.0f;
        }
        for (std::size_t j = 0; j < input.size(); j++) {
            if (input.at(j).correState == NONE || input.at(j).correState == WITH_POINT) {
                phi_increament = fabs(points.at(i).pitch_angle - input.at(j).pitch_angle);
                azimuth_increament = fabs(points.at(i).azi_angle - input.at(j).azi_angle);
                distance_increament = sqrt(powf(input.at(j).pos_x - points.at(i).pos_x, 2.0f)
                                           + powf(input.at(j).pos_y - points.at(i).pos_y, 2.0f) +
                                           powf(input.at(j).pos_z - points.at(i).pos_z, 2.0f));

                doppler_change = fabs((int) (input.at(j).doppler - points.at(i).doppler));
                range_increament = sqrt(powf(input.at(j).pos_x, 2.0f)
                                        + powf(input.at(j).pos_y, 2.0f) +
                                        powf(input.at(j).pos_z, 2.0f)) - point_r;
                v_abs = abs(range_increament / dt);
                float r_pre = (input.at(j).doppler - doppler_max / 2 + points.at(i).doppler - doppler_max / 2) / 2 *
                              doppler_res * dt;
                range_increament = abs(range_increament - r_pre);
                //TODO: 点击关联
                if (range_increament < range_threshold
                    && azimuth_increament < azimuth_threshold
                    && phi_increament < phi_threshold
                    && doppler_change < doppler_threshold_max
                    && v_abs < v_TH
                    //&& direction_threshold >=0
                        ) {
                    if (elem_topush.associated_count >= this->m_max_associated_count) {
                        std::cout << "超出最大关联点数" << std::endl;
                    } else {
                        input.at(j).correState = WITH_POINT;
                        elem_topush.associated_count += 1;
                        elem_topush.point_id[elem_topush.associated_count - 1] = j;
                    }
                }
            }
        }
        if (elem_topush.associated_count > 0) {
            output.push_back(elem_topush);
            points.at(i).isUpdated = true;
            points.at(i).associate_count += 1;
        }
    }
}

void DataProcessing::update_active_track_state(std::vector<TrackInfo> &track) {
    for (std::size_t i = 0; i < track.size(); i++) {
        if (track.at(i).isUpdated) {
            if (track.at(i).deletion_count > 0)
                track.at(i).deletion_count = 0;
        } else {
            track.at(i).deletion_count += 1;
            if (track.at(i).deletion_count >= track.at(i).deletion_threshold)
                track.at(i).track_state = EXPIRED_TRACK;
        }
    }
}

void DataProcessing::update_detective_track_state(std::vector<TrackInfo> &track) {
    for (std::size_t i = 0; i < track.size(); i++) {
        if (track.at(i).isUpdated) {
            track.at(i).deletion_count = 0;
            track.at(i).start_count += 1;
            //从点迹中关联到形成稳态航迹一共要五个点
            if (track.at(i).start_count >= track.at(i).start_threshold)
                track.at(i).track_state = ACTIVE_TRACK;
            else if (track.at(i).start_count < 0)
                track.at(i).start_count = 1;
        } else {
            track.at(i).deletion_count += 1;
            if (track.at(i).deletion_count >= track.at(i).deletion_threshold)
                track.at(i).track_state = EXPIRED_TRACK;
            else {
                if (track.at(i).start_count > 0)
                    track.at(i).start_count -= 1;
            }
        }
    }
}

void DataProcessing::update_points_state(std::vector<PointTemp> &input) {
    for (uint32_t i = 0; i < input.size(); i++) {
        if (input.at(i).isUpdated) {
            input.at(i).state = DETECTIVE_TRACK;
#if 0
            if (input.at(i).associate_count > 0)
                input.at(i).state = DETECTIVE_TRACK;
            else
                input.at(i).associate_count += 1;
#endif
        } else {
            if (input.at(i).state == -1 && input.at(i).three_circle_jishu == 3) {
                input.at(i).state = EXPIRED_POINT;
            } else if (input.at(i).three_circle_jishu < 3) {
                input.at(i).three_circle_jishu += 1;
            }

        }
    }
}

void DataProcessing::delete_expired_detective_track(std::vector<TrackInfo> &track) {
    std::vector<TrackInfo>::iterator iter = track.begin();
    while (iter != track.end()) {
        switch (iter->track_state) {
            case EXPIRED_TRACK:
                m_id_cache.push(iter->track_seq);
                iter = track.erase(iter);
                break;
            case ACTIVE_TRACK:
                iter = track.erase(iter);
                break;
            default:
                iter++;
                break;
        }
    }
}

void DataProcessing::delete_expired_active_track(std::vector<TrackInfo> &track) {
    std::vector<TrackInfo>::iterator iter = track.begin();
    while (iter != track.end()) {
        if (iter->track_state == EXPIRED_TRACK) {
            m_id_cache.push(iter->track_seq);
            iter = track.erase(iter);
        } else
            iter++;
    }
}

void DataProcessing::delete_expired_points(std::vector<PointTemp> &point) {
    std::vector<PointTemp>::iterator iter = point.begin();
    while (iter != point.end()) {
        m_id_cache.push(iter->point_seq);
        iter++;
    }
    point.clear();
}

void DataProcessing::active_track_filtering(std::vector<PointInfo> &input,
                                            std::vector<TrackInfo> &track,
                                            std::vector<associated_struct> &associated_list) {
    for (std::size_t i = 0; i < track.size(); i++) {
        if (track.at(i).isUpdated) {
            for (std::size_t j = 0; j < associated_list.size(); j++) {
                if (track.at(i).track_seq == associated_list.at(j).track_id)
                    unit_active_track_filtering(input.at(associated_list.at(j).point_id[0]), track.at(i));
            }
        }
    }
}

//TODO 稳态航迹滤波
void DataProcessing::unit_active_track_filtering(PointInfo &input, TrackInfo &track) {
    //iir_forget_factor 判断是否机动目标选择
    float iir_forget_factor = this->iir_forget_factor;
    if (track.isManeuverable)
        iir_forget_factor = 1.0f;


    //! 计算滤波误差协方差矩阵
    //Tracker_temp(tid).P_v = F_v * Tracker_temp(tid).P_v * F_v'+ Q_v;
    float Doppler_F_Trans[2 * 2], Doppler_PredictedErrorMatrix_Tmp[2 * 2], Doppler_product_AP[
            2 * 2], Doppler_product_APA[2 * 2];
    m_MatrixOprt->MatTrans(m_KalmanFilterParam.F_v, Doppler_F_Trans, 2, 2);
    m_MatrixOprt->Mats_multiply(m_KalmanFilterParam.F_v, track.doppler_filtered_error_matrix, Doppler_product_AP, 2, 2,
                                2);
    m_MatrixOprt->Mats_multiply(Doppler_product_AP, Doppler_F_Trans, Doppler_product_APA, 2, 2, 2);
    m_MatrixOprt->MatAdd(Doppler_product_APA, m_KalmanFilterParam.Q_v, Doppler_PredictedErrorMatrix_Tmp, 2, 2);
    for (int i = 0; i < 2; i++) {
        for (int j = 0; j < 2; j++) {
            track.doppler_filtered_error_matrix[i * 2 + j] =
                    0.5f * (Doppler_PredictedErrorMatrix_Tmp[i * 2 + j] + Doppler_PredictedErrorMatrix_Tmp[j * 2 + i]);
        }
    }

    //! 更新多普勒滤波值 && 机动判别
    float combine_v[2 * 1] = {(float) input.doppler, (float) (input.doppler - track.doppler_filtered[0]) / this->dt};
    //new 用当前关联点和航迹上一次的滤波值进行更新 不考虑是否发生机动
    memcpy(track.doppler_filtered, combine_v, sizeof(float) * 2);
    m_MatrixOprt->Mats_multiply(m_KalmanFilterParam.F_v, track.doppler_filtered, track.doppler_predicted, 2, 2, 1);

    //! kalman滤波
    //! 修正输入点的坐标并计算加速度
    float input_point[6 * 1] = {input.pos_x, input.pos_y, input.pos_z, (input.pos_x - track.filtered[0]) / dt,
                                (input.pos_y - track.filtered[1]) / dt, (input.pos_z - track.filtered[2]) / dt};


    //! 保留原始滤波值的信息并修正原始滤波值
    float X_predicted[6 * 1] = {track.predicted[0], track.predicted[1], track.predicted[2], track.predicted[3],
                                track.predicted[4], track.predicted[5]};
    float v_factor = 0;//速度权重
    for (int i = 3; i < 6; i++)
        track.predicted[i] = track.predicted[i] * (1.0f - v_factor) + input_point[i] * v_factor;

    //Eq1:更新预测误差协方差矩阵PredictedErrorMatrix
    float F_Trans[6 * 6], PredictedErrorMatrix[6 * 6], PredictedErrorMatrix_Tmp[6 * 6], product_AP[6 * 6], product_APA[
            6 * 6];
    m_MatrixOprt->MatTrans(m_KalmanFilterParam.F, F_Trans, 6, 6);
    m_MatrixOprt->Mats_multiply(m_KalmanFilterParam.F, track.filtered_error_matrix, product_AP, 6, 6, 6);
    m_MatrixOprt->Mats_multiply(product_AP, F_Trans, product_APA, 6, 6, 6);
    m_MatrixOprt->MatAdd(product_APA, m_KalmanFilterParam.Q, PredictedErrorMatrix_Tmp, 6, 6);
    for (int i = 0; i < 6; i++) {
        for (int j = 0; j < 6; j++) {
            PredictedErrorMatrix[i * 6 + j] =
                    0.5f * (PredictedErrorMatrix_Tmp[i * 6 + j] + PredictedErrorMatrix_Tmp[j * 6 + i]);
        }
    }
    track.range_0 = sqrt(powf(track.filtered[0], 2.0f) + powf(track.filtered[1], 2.0f) +
                         powf(track.filtered[2], 2.0f));
    if (track.isManeuverable) {
        //! Eq3:机动目标直接用输入点迹更新滤波值
        memcpy(track.filtered, input_point, sizeof(float) * 6);

        //! Eq4:滤波误差协方差矩阵
        float eye6[STATE_DEMENSION * STATE_DEMENSION] = {100, 0, 0, 0, 0, 0,
                                                         0, 100, 0, 0, 0, 0,
                                                         0, 0, 100, 0, 0, 0,
                                                         0, 0, 0, 100, 0, 0,
                                                         0, 0, 0, 0, 100, 0,
                                                         0, 0, 0, 0, 0, 100};
        memcpy(track.filtered_error_matrix, eye6, sizeof(float) * 6 * 6);

        //! Eq5:更新预测值
        float predicted_p2[6 * 1], predicted_p1[6 * 1];
        m_MatrixOprt->Mats_multiply(m_KalmanFilterParam.B, track.accumulation, predicted_p1, 6, 3, 1);
        m_MatrixOprt->Mats_multiply(m_KalmanFilterParam.F, track.filtered, predicted_p2, 6, 6, 1);
        m_MatrixOprt->MatAdd(predicted_p1, predicted_p2, track.predicted, 6, 1);

    } else {
        // 计算雅可比矩阵
        float kalman_kg[3 * 6];
        memset(m_KalmanFilterParam.H, 0, sizeof(float) * 3 * 6);
        memset(kalman_kg, 0, sizeof(float) * 3 * 6);
        getMatrixH(track.predicted[0], track.predicted[1], track.predicted[2], track.predicted[3], track.predicted[4],
                   track.predicted[5], m_KalmanFilterParam.H, 3, 6);

        //Eq2:计算滤波器增益kalman_kg
        float H_trans[6 * 3], product_PHT[6 * 3], product_HPHT[3 * 3], sum[3 * 3], kg_inv[3 * 3];
        m_MatrixOprt->MatTrans(m_KalmanFilterParam.H, H_trans, 3, 6);
        m_MatrixOprt->Mats_multiply(PredictedErrorMatrix, H_trans, product_PHT, 6, 6, 3);//PH'
        m_MatrixOprt->Mats_multiply(m_KalmanFilterParam.H, product_PHT, product_HPHT, 3, 6, 3);//HPH'
        m_MatrixOprt->MatAdd(product_HPHT, m_KalmanFilterParam.R, sum, 3, 3);
        m_MatrixOprt->Matinv(sum, kg_inv, 3);
        m_MatrixOprt->Mats_multiply(product_PHT, kg_inv, kalman_kg, 6, 3, 3);

        //Eq4:更新误差协方差矩阵
        float product_KgH[6 * 6], des_IKgH[6 * 6];
        float eye[6 * 6] = {1, 0, 0, 0, 0, 0,
                            0, 1, 0, 0, 0, 0,
                            0, 0, 1, 0, 0, 0,
                            0, 0, 0, 1, 0, 0,
                            0, 0, 0, 0, 1, 0,
                            0, 0, 0, 0, 0, 1};
        m_MatrixOprt->Mats_multiply(kalman_kg, m_KalmanFilterParam.H, product_KgH, 6, 3, 6);
        m_MatrixOprt->MatDec(eye, product_KgH, des_IKgH, 6, 6);
        m_MatrixOprt->Mats_multiply(des_IKgH, PredictedErrorMatrix, track.filtered_error_matrix, 6, 6, 6);

        //Eq3:计算滤波值X_filtered
        float temp = sqrt(powf(X_predicted[0], 2.0f) + powf(X_predicted[1], 2.0f) + powf(X_predicted[2], 2.0f));
        float diff[3 * 1], product_Kgz[6 * 1];
        diff[0] = sqrt(powf(input.pos_x, 2.0f) + powf(input.pos_y, 2.0f) + powf(input.pos_z, 2.0f)) -
                  sqrt(powf(X_predicted[0], 2.0f) + powf(X_predicted[1], 2.0f) + powf(X_predicted[2], 2.0f));//x-hx
        float a = atan2(input.pos_y, input.pos_x) / pi * 180;
        float b = atan2(X_predicted[1], X_predicted[0]) / pi * 180;
        diff[1] = atan2(input.pos_y, input.pos_x) - atan2(X_predicted[1], X_predicted[0]);
        diff[2] = input.pitch_angle * pi / 180 -
                  atan2(X_predicted[2], sqrt(powf(X_predicted[0], 2.0f) + powf(X_predicted[1], 2.0f)));
        m_MatrixOprt->Mats_multiply(kalman_kg, diff, product_Kgz, 6, 3, 1);
        m_MatrixOprt->MatAdd(track.predicted, product_Kgz, track.filtered, 6, 1);
        float c = atan2(track.filtered[1], track.filtered[0]) / pi * 180;

        //Eq5:更新预测值
        float predicted_p2[6 * 1], predicted_p1[6 * 1];
        m_MatrixOprt->Mats_multiply(m_KalmanFilterParam.B, track.accumulation, predicted_p1, 6, 3, 1);
        m_MatrixOprt->Mats_multiply(m_KalmanFilterParam.F, track.filtered, predicted_p2, 6, 6, 1);
        m_MatrixOprt->MatAdd(predicted_p1, predicted_p2, track.predicted, 6, 1);

    }

    track.velocity = input.velocity;
    track.distance_increament = fabs(input.velocity) * dt;
    track.dotbianhao += 1;
    track.updated_count += 1;
    track.SNR = input.SNR;
    track.noise = input.noise;
//    track.pitchAngle = input.pitch_angle;
//    track.azi_angle = input.azi_angle;
    //改变一下，上报成与上一次相差1.12s
    float million_second = input.hour * 3600 + input.minute * 60 + input.second;
    track.hour = short(million_second / 3600);
    track.minute = short((int(million_second) % 3600) / 60);
    track.second = short((int(million_second) % 3600) % 60 + 0.5);

    track.biaozhi = input.biaozhi;


}

void DataProcessing::detective_track_filtering(std::vector<PointInfo> &input,
                                               std::vector<TrackInfo> &track,
                                               std::vector<associated_struct> &associated_list) {
    for (std::size_t i = 0; i < track.size(); i++) {
        if (track.at(i).isUpdated) {
            for (std::size_t j = 0; j < associated_list.size(); j++) {
                if (track.at(i).track_seq == associated_list.at(j).track_id) {
                    unit_detective_track_filtering(input.at(associated_list.at(j).point_id[0]), track.at(i));//tested
                }
            }
        }
    }
}

//TODO 暂态航迹滤波
void DataProcessing::unit_detective_track_filtering(PointInfo &input, TrackInfo &track) {
    float iir_forget_factor = this->iir_forget_factor;
    float a_inc_x = this->m_a_inc_x;
    float a_inc_y = this->m_a_inc_y;

    track.range_0 = sqrt(powf(track.filtered[0], 2.0f) + powf(track.filtered[1], 2.0f) +
                         powf(track.filtered[2], 2.0f));
    //计算多普勒滤波误差协方差矩阵
    //Tracker_temp(tid).P_v = F_v * Tracker_temp(tid).P_v * F_v'+ Q_v;
    float Doppler_F_Trans[2 * 2], Doppler_PredictedErrorMatrix_Tmp[2 * 2], Doppler_product_AP[
            2 * 2], Doppler_product_APA[2 * 2];
    m_MatrixOprt->MatTrans(m_KalmanFilterParam.F_v, Doppler_F_Trans, 2, 2);
    m_MatrixOprt->Mats_multiply(m_KalmanFilterParam.F_v, track.doppler_filtered_error_matrix, Doppler_product_AP, 2, 2,
                                2);
    m_MatrixOprt->Mats_multiply(Doppler_product_AP, Doppler_F_Trans, Doppler_product_APA, 2, 2, 2);
    m_MatrixOprt->MatAdd(Doppler_product_APA, m_KalmanFilterParam.Q_v, Doppler_PredictedErrorMatrix_Tmp, 2, 2);
    for (int i = 0; i < 2; i++) {
        for (int j = 0; j < 2; j++) {
            track.doppler_filtered_error_matrix[i * 2 + j] =
                    0.5f * (Doppler_PredictedErrorMatrix_Tmp[i * 2 + j] + Doppler_PredictedErrorMatrix_Tmp[j * 2 + i]);
        }
    }

    //计算多普勒滤波值
    float combine_v[2 * 1] = {(float) input.doppler, (float) (input.doppler - track.doppler_filtered[0]) / dt};
    //new
    memcpy(track.doppler_filtered, combine_v, sizeof(float) * 2);


    //计算多普勒预测值
    //Tracker(tid).dop_ini_pred = F_v*Tracker(tid).dop_ini;
    m_MatrixOprt->Mats_multiply(m_KalmanFilterParam.F_v, track.doppler_filtered, track.doppler_predicted, 2, 2, 1);


    //! kalman滤波
    //! 修正输入点的坐标并计算加速度
    float input_point[6 * 1] = {input.pos_x, input.pos_y, input.pos_z, (input.pos_x - track.filtered[0]) / dt,
                                (input.pos_y - track.filtered[1]) / dt, (input.pos_z - track.filtered[2]) / dt};

    float vx_temp = (input_point[0] - track.filtered[0]) / dt;
    float vy_temp = (input_point[1] - track.filtered[1]) / dt;
    float vz_temp = (input_point[2] - track.filtered[2]) / dt;

    track.accumulation[0] = 0.0f;
    track.accumulation[1] = 0.0f;
    track.accumulation[2] = 0.0f;


    //! 保存原始滤波值并修正原始滤波值
    float X_predicted[6 * 1] = {track.predicted[0], track.predicted[1], track.predicted[2], track.predicted[3],
                                track.predicted[4], track.predicted[5]};
    for (int i = 0; i < 6; i++)
        track.predicted[i] = track.predicted[i] * (1.0f - iir_forget_factor) + input_point[i] * iir_forget_factor;

    //! 更新矩阵中的R/H
    float kalman_kg[3 * 6];
    memset(m_KalmanFilterParam.H, 0, sizeof(float) * 3 * 6);
    memset(kalman_kg, 0, sizeof(float) * 3 * 6);
    getMatrixH(track.predicted[0], track.predicted[1], track.predicted[2], track.predicted[3], track.predicted[4],
               track.predicted[5], m_KalmanFilterParam.H, 3, 6);
#if 0
    memset(m_KalmanFilterParam.R, 0, sizeof(float) * 2 * 2);
    m_KalmanFilterParam.R[0] = RangeVar * m_KalmanFilterParam.measurement_noise_var;
    m_KalmanFilterParam.R[3] = AngleVar * m_KalmanFilterParam.measurement_noise_var;
#endif

    //Eq1:更新预测误差协方差矩阵PredictedErrorMatrix
    float F_Trans[6 * 6], PredictedErrorMatrix[6 * 6], PredictedErrorMatrix_Tmp[6 * 6], product_AP[6 * 6], product_APA[
            6 * 6];
    m_MatrixOprt->MatTrans(m_KalmanFilterParam.F, F_Trans, 6, 6);
    m_MatrixOprt->Mats_multiply(m_KalmanFilterParam.F, track.filtered_error_matrix, product_AP, 6, 6, 6);
    m_MatrixOprt->Mats_multiply(product_AP, F_Trans, product_APA, 6, 6, 6);
    m_MatrixOprt->MatAdd(product_APA, m_KalmanFilterParam.Q, PredictedErrorMatrix_Tmp, 6, 6);
    for (int i = 0; i < 6; i++) {
        for (int j = 0; j < 6; j++) {
            PredictedErrorMatrix[i * 6 + j] =
                    0.5f * (PredictedErrorMatrix_Tmp[i * 6 + j] + PredictedErrorMatrix_Tmp[j * 6 + i]);
        }
    }


    //Eq2:计算滤波器增益kalman_kg
    float H_trans[6 * 3], product_PHT[6 * 3], product_HPHT[3 * 3], sum[3 * 3], kg_inv[3 * 3];
    m_MatrixOprt->MatTrans(m_KalmanFilterParam.H, H_trans, 3, 6);
    m_MatrixOprt->Mats_multiply(PredictedErrorMatrix, H_trans, product_PHT, 6, 6, 3);//PH'
    m_MatrixOprt->Mats_multiply(m_KalmanFilterParam.H, product_PHT, product_HPHT, 3, 6, 3);//HPH'
    m_MatrixOprt->MatAdd(product_HPHT, m_KalmanFilterParam.R, sum, 3, 3);
    m_MatrixOprt->Matinv(sum, kg_inv, 3);
    m_MatrixOprt->Mats_multiply(product_PHT, kg_inv, kalman_kg, 6, 3, 3);

    float product_KgH[6 * 6], des_IKgH[6 * 6];
    float eye[6 * 6] = {1, 0, 0, 0, 0, 0,
                        0, 1, 0, 0, 0, 0,
                        0, 0, 1, 0, 0, 0,
                        0, 0, 0, 1, 0, 0,
                        0, 0, 0, 0, 1, 0,
                        0, 0, 0, 0, 0, 1};
    m_MatrixOprt->Mats_multiply(kalman_kg, m_KalmanFilterParam.H, product_KgH, 6, 3, 6);
    m_MatrixOprt->MatDec(eye, product_KgH, des_IKgH, 6, 6);
    m_MatrixOprt->Mats_multiply(des_IKgH, PredictedErrorMatrix, track.filtered_error_matrix, 6, 6, 6);

    //Eq3:计算滤波值X_filtered
    float diff[3 * 1], product_Kgz[6 * 1];
    diff[0] = sqrt(powf(input.pos_x, 2.0f) + powf(input.pos_y, 2.0f) + powf(input.pos_z, 2.0f)) -
              sqrt(powf(X_predicted[0], 2.0) + powf(X_predicted[1], 2.0) + powf(X_predicted[2], 2.0));//x-hx
    float a = atan2(X_predicted[1], X_predicted[0]) * 180 / pi;
    float b = atan2(X_predicted[2], sqrt(powf(X_predicted[0], 2.0f) + powf(X_predicted[1], 2.0f))) * 180 / pi;
    diff[1] = input.azi_angle / 180 * pi - atan2(X_predicted[1], X_predicted[0]);
    diff[2] = input.pitch_angle / 180 * pi -
              atan2(X_predicted[2], sqrt(powf(X_predicted[0], 2.0f) + powf(X_predicted[1], 2.0f)));

    m_MatrixOprt->Mats_multiply(kalman_kg, diff, product_Kgz, 6, 3, 1);
    m_MatrixOprt->MatAdd(track.predicted, product_Kgz, track.filtered, 6, 1);


    //Eq4:滤波误差协方差矩阵


    //Eq5:更新预测值
    float predicted_p2[6 * 1], predicted_p1[6 * 1];
    m_MatrixOprt->Mats_multiply(m_KalmanFilterParam.B, track.accumulation, predicted_p1, 6, 3, 1);
    m_MatrixOprt->Mats_multiply(m_KalmanFilterParam.F, track.filtered, predicted_p2, 6, 6, 1);
    m_MatrixOprt->MatAdd(predicted_p1, predicted_p2, track.predicted, 6, 1);

    //! 其他
    track.velocity = sqrt(
            powf(track.filtered[3], 2.0f) + powf(track.filtered[4], 2.0f) + powf(track.filtered[5], 2.0f));
    track.distance_increament = fabs(input.velocity) * dt;

    track.dotbianhao = track.dotbianhao;    //new 暂态航迹不对dotbianhao操作
    track.updated_count += 1;
    track.noise = input.noise;

    track.pitchAngle = input.pitch_angle;
    track.azi_angle = input.azi_angle;
    track.SNR = input.SNR;

    //改变一下，上报成与上一次相差1.12s
    float million_second = input.hour * 3600 + input.minute * 60 + input.second;
    track.hour = short(million_second / 3600);
    track.minute = short((int(million_second) % 3600) / 60);
    track.second = short((int(million_second) % 3600) % 60 + 0.5);

    track.biaozhi = input.biaozhi;
}

void DataProcessing::active_track_filtering_without_update(std::vector<TrackInfo> &track) {
    for (std::size_t i = 0; i < track.size(); i++) {
        if (!track.at(i).isUpdated) {
//            std::cout << "track_id=" << track.at(i).track_seq << " budianflag=" << 0 << std::endl;
            track.at(i).budianflag = true;

#if printf
            std::cout << std::endl;
            std::cout << "track_id=" << track.at(i).track_seq << " budianflag=" << 0 << std::endl;
            std::cout << "last cirlce filtered x=" << track.at(i).filtered[0] << "   last cirlce filtered y=" << track.at(i).filtered[1] << std::endl;
            std::cout << "last cirlce predicted x=" << track.at(i).predicted[0] << "   last cirlce predicted y=" << track.at(i).predicted[1] << std::endl;
#endif

            track.at(i).range_0 = sqrt(powf(track.at(i).filtered[0], 2.0f) + powf(track.at(i).filtered[1], 2.0f) +
                                       powf(track.at(i).filtered[2], 2.0f));
            //更新预测误差协方差矩阵PredictedErrorMatrix
            float F_Trans[6 * 6], product_AP[6 * 6], product_APA[6 * 6];
            m_MatrixOprt->MatTrans(m_KalmanFilterParam.F, F_Trans, 6, 6);
            m_MatrixOprt->Mats_multiply(m_KalmanFilterParam.F, track.at(i).filtered_error_matrix, product_AP, 6, 6, 6);
            m_MatrixOprt->Mats_multiply(product_AP, F_Trans, product_APA, 6, 6, 6);
            m_MatrixOprt->MatAdd(product_APA, m_KalmanFilterParam.Q, track.at(i).filtered_error_matrix, 6, 6);

            //更新滤波值
            memcpy(track.at(i).filtered, track.at(i).predicted, sizeof(float) * 6);

            //更新预测值
            float pred_1[6 * 1], pred_2[6 * 1];
            m_MatrixOprt->Mats_multiply(m_KalmanFilterParam.F, track.at(i).filtered, pred_1, 6, 6, 1);
            m_MatrixOprt->Mats_multiply(m_KalmanFilterParam.B, track.at(i).accumulation, pred_2, 6, 3, 1);
            m_MatrixOprt->MatAdd(pred_1, pred_2, track.at(i).predicted, 6, 1);

            //Tracker(tid).P_v = F_v * Tracker(tid).P_v * F_v'+ Q_v;
            float Doppler_F_Trans[2 * 2], Doppler_product_AP[2 * 2], Doppler_product_APA[2 * 2];
            m_MatrixOprt->MatTrans(m_KalmanFilterParam.F_v, Doppler_F_Trans, 2, 2);
            m_MatrixOprt->Mats_multiply(m_KalmanFilterParam.F_v, track.at(i).doppler_filtered_error_matrix,
                                        Doppler_product_AP, 2, 2, 2);
            m_MatrixOprt->Mats_multiply(Doppler_product_AP, Doppler_F_Trans, Doppler_product_APA, 2, 2, 2);
            m_MatrixOprt->MatAdd(Doppler_product_APA, m_KalmanFilterParam.Q_v,
                                 track.at(i).doppler_filtered_error_matrix, 2, 2);

            //float dop_ini_pred[2 * 1];
            memcpy(track.at(i).doppler_filtered, track.at(i).doppler_predicted, sizeof(float) * 2);
            m_MatrixOprt->Mats_multiply(m_KalmanFilterParam.F_v, track.at(i).doppler_filtered,
                                        track.at(i).doppler_predicted, 2, 2, 1);

            track.at(i).dotbianhao -= 1;  //new 稳态航迹补点，dotbianhao-1
            track.at(i).unupdated_count += 1;
            track.at(i).velocity = sqrt(powf(track.at(i).predicted[2], 2.0f) + powf(track.at(i).predicted[3], 2.0f));
            // track.at(i).velocity = track.at(i).velocity / fabs(track.at(i).velocity) * sqrt(powf(track.at(i).predicted[2], 2.0f) + powf(track.at(i).predicted[3], 2.0f));
            // if (track.at(i).velocity > 0)
            // 	track.at(i).velocity = sqrt(powf(track.at(i).predicted[2], 2.0f) + powf(track.at(i).predicted[3], 2.0f));
            // else
            // 	track.at(i).velocity = (-1.0f) * sqrt(powf(track.at(i).predicted[2], 2.0f) + powf(track.at(i).predicted[3], 2.0f));

#if printf
            std::cout << "this cirlce filtered x=" << track.at(i).filtered[0] << "   this cirlce filtered y=" << track.at(i).filtered[1] << std::endl;
            std::cout << "this cirlce Vx=" << track.at(i).filtered[2] << "    this cirlce Vy=" << track.at(i).filtered[3] << std::endl;
#endif
            //预测的时间改变一下，上报成与上一次相差1.12s
            float million_second = track.at(i).hour * 3600 + track.at(i).minute * 60 + track.at(i).second;
            million_second = million_second + 2.38;
            track.at(i).hour = short(million_second / 3600);
            track.at(i).minute = short((int(million_second) % 3600) / 60);
            track.at(i).second = short((int(million_second) % 3600) % 60 + 0.5);

            track.at(i).biaozhi = 1.0f * track.at(i).biaozhi;


        }
    }
}

void DataProcessing::detective_track_filtering_without_update(std::vector<TrackInfo> &track) {
    for (std::size_t i = 0; i < track.size(); i++) {
        if (!track.at(i).isUpdated) {
            track.at(i).range_0 = sqrt(powf(track.at(i).filtered[0], 2.0f) + powf(track.at(i).filtered[1], 2.0f) +
                                       powf(track.at(i).filtered[2], 2.0f));
            //更新预测误差协方差矩阵PredictedErrorMatrix
            float F_Trans[6 * 6], product_AP[6 * 6], product_APA[6 * 6];
            m_MatrixOprt->MatTrans(m_KalmanFilterParam.F, F_Trans, 6, 6);
            m_MatrixOprt->Mats_multiply(m_KalmanFilterParam.F, track.at(i).filtered_error_matrix, product_AP, 6, 6, 6);
            m_MatrixOprt->Mats_multiply(product_AP, F_Trans, product_APA, 6, 6, 6);
            m_MatrixOprt->MatAdd(product_APA, m_KalmanFilterParam.Q, track.at(i).filtered_error_matrix, 6, 6);

            //更新滤波值
            memcpy(track.at(i).filtered, track.at(i).predicted, sizeof(float) * 6);

            //更新预测值
            float pred_1[6 * 1], pred_2[6 * 1];
            m_MatrixOprt->Mats_multiply(m_KalmanFilterParam.F, track.at(i).filtered, pred_1, 6, 6, 1);
            m_MatrixOprt->Mats_multiply(m_KalmanFilterParam.B, track.at(i).accumulation, pred_2, 6, 3, 1);
            m_MatrixOprt->MatAdd(pred_1, pred_2, track.at(i).predicted, 6, 1);

            //更新多普勒预测误差协方差矩阵PredictedErrorMatrix
            float Doppler_F_Trans[2 * 2], Doppler_product_AP[2 * 2], Doppler_product_APA[2 * 2];
            m_MatrixOprt->MatTrans(m_KalmanFilterParam.F_v, Doppler_F_Trans, 2, 2);
            m_MatrixOprt->Mats_multiply(m_KalmanFilterParam.F_v, track.at(i).doppler_filtered_error_matrix,
                                        Doppler_product_AP, 2, 2, 2);
            m_MatrixOprt->Mats_multiply(Doppler_product_AP, Doppler_F_Trans, Doppler_product_APA, 2, 2, 2);
            m_MatrixOprt->MatAdd(Doppler_product_APA, m_KalmanFilterParam.Q_v,
                                 track.at(i).doppler_filtered_error_matrix, 2, 2);

            //更新多普勒预测值
            memcpy(track.at(i).doppler_filtered, track.at(i).doppler_predicted, sizeof(float) * 2);
            m_MatrixOprt->Mats_multiply(m_KalmanFilterParam.F_v, track.at(i).doppler_filtered,
                                        track.at(i).doppler_predicted, 2, 2, 1);

            track.at(i).dotbianhao = track.at(i).dotbianhao;  //new 暂态航迹补点，不对dotbianhao进行操作
            track.at(i).unupdated_count += 1;
            track.at(i).velocity = sqrt(powf(track.at(i).predicted[3], 2.0f) + powf(track.at(i).predicted[4], 2.0f) +
                                        powf(track.at(i).predicted[5], 2.0f));
            // if (track.at(i).velocity > 0)
            // 	track.at(i).velocity = sqrt(powf(track.at(i).predicted[2], 2.0f) + powf(track.at(i).predicted[3], 2.0f));
            // else
            // 	track.at(i).velocity = (-1.0f) * sqrt(powf(track.at(i).predicted[2], 2.0f) + powf(track.at(i).predicted[3], 2.0f));

            //预测的时间改变一下，上报成和上一次相差1.12s
            int million_second = track.at(i).hour * 3600 + track.at(i).minute * 60 + track.at(i).second;
            million_second = million_second + 2.38;
            track.at(i).hour = short(million_second / 3600);
            track.at(i).minute = short((million_second % 3600) / 60);
            track.at(i).second = short((million_second % 3600) % 60 + 0.5);
        }
    }
}

int DataProcessing::init_map() {
    int rtn = 0;
    std::pair<uint32_t, std::vector<TrackInfo>> elem_topush;
    std::pair<std::map<uint32_t, std::vector<TrackInfo>>::iterator, bool> insert_pair;
    std::vector<TrackInfo> track_topush;
    track_topush.clear();
    for (uint32_t i = 0; i < sector_count; i++) {
        elem_topush.first = i;
        elem_topush.second = track_topush;
        insert_pair = m_all_active_track.insert(elem_topush);
        if (!insert_pair.second) {
            rtn = -1;
            std::cout << "m_all_active_track insert failed!\n";
            break;
        }
        insert_pair = m_all_detective_track.insert(elem_topush);
        if (!insert_pair.second) {
            rtn = -1;
            std::cout << "m_all_detective_track insert failed!\n";
            break;
        }
    }
    return rtn;
}

void DataProcessing::getMatrixH(float pos_x, float pos_y, float pos_z, float vel_x, float vel_y, float vel_z, float *H,
                                int row, int column) {
    memset(H, 0, sizeof(float) * row * column);
    float r = sqrt(powf(pos_x, 2.0) + powf(pos_y, 2.0) + powf(pos_z, 2.0));
    float r2 = powf(pos_x, 2.0) + powf(pos_y, 2.0) + powf(pos_z, 2.0);

    float r12 = powf(pos_x, 2.0) + powf(pos_y, 2.0);
    float r1 = sqrt(r12);


    H[0] = pos_x / r;
    H[1] = pos_y / r;
    H[2] = pos_z / r;

    H[column * 1 + 0] = -pos_y / r12;
    H[column * 1 + 1] = pos_x / r12;
    H[column * 1 + 2] = 0;

    H[column * 2 + 0] = -pos_x * pos_z / r1 / r2;
    H[column * 2 + 1] = -pos_y * pos_z / r1 / r2;
    H[column * 2 + 2] = r1 / r2;
}

//TODO:数据处理开始 processing per circle
void
DataProcessing::processing_per_circle(std::vector<PointInfo> &input, std::vector<TrackOut> &output, int circle_num) {
    initialize();
    circle = circle_num;
    correlate_with_active_tracks(input, m_active_track_vec, m_active_associated_list);
    correlate_with_detective_tracks(input, m_detective_track_vec, m_detective_associated_list);

    resolute_ambiguity(m_active_track_vec, m_active_associated_list, m_detective_track_vec,
                       m_detective_associated_list);
    update_active_track_state(m_active_track_vec);
    delete_expired_active_track(m_active_track_vec);

    active_track_filtering(input, m_active_track_vec, m_active_associated_list);
    active_track_filtering_without_update(m_active_track_vec);
    detective_track_filtering(input, m_detective_track_vec, m_detective_associated_list);
    detective_track_filtering_without_update(m_detective_track_vec);


    update_detective_track_state(m_detective_track_vec);
    start_active_track(m_detective_track_vec, m_detective_associated_list,
                       m_active_track_vec, m_active_associated_list);
    delete_expired_detective_track(m_detective_track_vec);

    correlate_with_points(input, m_points_vec, m_points_associated_list);
    update_points_state(m_points_vec);
    start_detective_track(input, m_points_vec, m_points_associated_list, m_detective_track_vec);
    delete_expired_points(m_points_vec);

    append_new_points(input, m_points_vec);

    output_the_track(m_active_track_vec, output);
}

void DataProcessing::output_the_track(std::vector<TrackInfo> &track, std::vector<TrackOut> &output) {
    TrackOut elem_topush;
    float azimuth = 0.0;
    float pitch = 0.0;
    float range = 0.0f;
    float range_change = 0.0f;
    float range_TH = 0.9f;
    float dop_ave = 0.0f;
    float r_dop_ave = 0.0f;
    float r_dop_change = 0.0f;
    float r_dop_TH = 2 * doppler_res * dt;
//TODO:数据处理输出
    for (std::size_t i = 0; i < track.size(); i++) {
        range = sqrt(powf(track.at(i).filtered[0], 2) + powf(track.at(i).filtered[1], 2)
                     + powf(track.at(i).filtered[2], 2));
        if (track.at(i).track_state != ACTIVE_TRACK) continue;
        if (track.at(i).dotbianhao == track.at(i).start_count + 2) {
            range_change = fabs(track.at(i).range_0 - range);
            dop_ave = (track.at(i).dopp_0 + track.at(i).doppler_filtered[0]) / 2;
            r_dop_ave = fabs(dop_ave - doppler_max / 2) * doppler_res * dt;
            r_dop_change = fabs(range_change - r_dop_ave);

            if (range > track.at(i).range_0 && track.at(i).doppler_filtered[0] > doppler_max / 2) {
                if (range_change > range_TH && r_dop_change < r_dop_TH) {
                    track.at(i).out_flag = 1;
                }
            } else if (range < track.at(i).range_0 && track.at(i).doppler_filtered[0] < doppler_max / 2) {
                if (range_change > range_TH && r_dop_change < r_dop_TH) {
                    track.at(i).out_flag = 1;
                }
            }
        } else if (track.at(i).dotbianhao >= 6) {
            track.at(i).out_flag = 1;
        }
        if (track.at(i).dotbianhao >= 3) {
            if (track.at(i).out_flag) {
                track.at(i).dotbianhao_flag = 1;
            } else {
                track.at(i).track_state = EXPIRED_TRACK;
                track.at(i).dotbianhao_flag = 0;
            }
        }
        if (track.at(i).dotbianhao_flag && track.at(i).out_flag) {
            track.at(i).dotbianhao_flag = 0;
            //动态调整消亡门限
            if (track.at(i).dotbianhao > 10)
                track.at(i).deletion_threshold = 6;

            int receive_id = this->active_id;
            //if ((track.at(i).isUpdated) && (track.at(i).track_seq==receive_id))
            if (track.at(i).isUpdated)
                elem_topush.point_type = 0;
            else
                elem_topush.point_type = 1;
            elem_topush.SNR = track.at(i).SNR;
            elem_topush.noise = track.at(i).noise;


#if CHAHZI
            //上报给显示的点迹是插值之后的模拟整数倍时间的结果
#else
            //上报给显示的点迹是直接四舍五入的原始结果
            azimuth = atan2(track.at(i).filtered[1], track.at(i).filtered[0]);   //atan2函数,区别于atan函数，在第二象限的范围是（pi/2，pi）
            azimuth = azimuth * 180 / pi;  //将方位转化为度，以x轴为0度
            elem_topush.Track_Azimuth = azimuth;

            pitch = atan2(track.at(i).filtered[2],
                          sqrt(powf(track.at(i).filtered[0], 2.0f) + powf(track.at(i).filtered[1], 2.0f))) * 180 / pi;
            elem_topush.pitchAngle = pitch;
            float temp = track.at(i).pitchAngle;
            elem_topush.track_deletion_threshold = track.at(i).deletion_count;
            elem_topush.Track_ID = track.at(i).track_seq;
            float range_change = 0.0f;
            elem_topush.Track_Range = sqrt(powf(track.at(i).filtered[0], 2.0f) + powf(track.at(i).filtered[1], 2.0f)
                                           + powf(track.at(i).filtered[2], 2.0f));
            elem_topush.Track_Velocity = sqrt(
                    powf(track.at(i).filtered[3], 2.0f) + powf(track.at(i).filtered[4], 2.0f) +
                    powf(track.at(i).filtered[5], 2.0f));

#endif

            if (track.at(i).doppler_filtered[0] < 32) {
                //远离雷达
                elem_topush.Track_Velocity = 1.0f * elem_topush.Track_Velocity;
            } else {
                //靠近雷达
                elem_topush.Track_Velocity = (-1.0f) * elem_topush.Track_Velocity;
            }

            elem_topush.unupdated_cnt = track.at(i).unupdated_count;
            elem_topush.updated_cnt = track.at(i).updated_count;
            elem_topush.year = track.at(i).year;
            elem_topush.month = track.at(i).month;
            elem_topush.day = track.at(i).day;
            elem_topush.hour = track.at(i).hour;
            elem_topush.minute = track.at(i).minute;
            elem_topush.second = track.at(i).second;
            elem_topush.active_id = receive_id;
            elem_topush.deletion_count = track.at(i).deletion_count;
            elem_topush.dotbianhao = track.at(i).dotbianhao;

            switch (track.at(i).track_state) {
                case DETECTIVE_TRACK:
                    elem_topush.track_type = 0;
                    break;
                case ACTIVE_TRACK:
                    elem_topush.track_type = 1;
                    break;
                default:
                    break;
            }
            /*if (track.at(i).track_seq == receive_id)
            {*/
            output.push_back(elem_topush);
            /*	}*/
        }

    }
}

void DataProcessing::start_active_track(std::vector<TrackInfo> &detective_track,
                                        std::vector<associated_struct> &detective_asslist,
                                        std::vector<TrackInfo> &active_track,
                                        std::vector<associated_struct> &active_asslist) {
    for (std::size_t i = 0; i < detective_track.size(); i++) {
        if (detective_track.at(i).track_state == ACTIVE_TRACK) {
            active_track.push_back(detective_track.at(i));
            active_track.at(active_track.size() - 1).deletion_threshold = 3;
            active_track.at(active_track.size() - 1).dotbianhao = detective_track.at(i).start_count + 2;
            // new  记录稳态航迹初始帧的多普勒和径向距离
            active_track.at(active_track.size() - 1).dopp_0 = detective_track.at(i).doppler_filtered[0];
            active_track.at(active_track.size() - 1).dotbianhao_flag = 0;
            active_track.at(active_track.size() - 1).out_flag = 0;
        }
    }
}

void DataProcessing::start_detective_track(
        std::vector<PointInfo> &input,
        std::vector<PointTemp> &points,
        std::vector<associated_struct> &associated_list,
        std::vector<TrackInfo> &track
) {
    float eye2[DOPPLER_DEMENSION * DOPPLER_DEMENSION] = {1, 0, 0, 1};
    float eye6[STATE_DEMENSION * STATE_DEMENSION] = {100, 0, 0, 0, 0, 0,
                                                     0, 100, 0, 0, 0, 0,
                                                     0, 0, 100, 0, 0, 0,
                                                     0, 0, 0, 100, 0, 0,
                                                     0, 0, 0, 0, 100, 0,
                                                     0, 0, 0, 0, 0, 100};
    TrackInfo elem_topush;
    float pos_x = 0.0f;
    float pos_y = 0.0f;
    float pos_z = 0.0f;
    float time_interval = 0.0f;
    for (std::size_t i = 0; i < points.size(); i++) {
        if (points.at(i).state == DETECTIVE_TRACK) {
            for (std::size_t j = 0; j < associated_list.size(); j++) {
                if (associated_list.at(j).track_id == points.at(i).point_seq) {
                    for (std::size_t k = 0; k < associated_list.at(j).associated_count; k++) {
                        if (k == 0) {
                            elem_topush.track_seq = points.at(i).point_seq;
                            time_interval =
                                    dt * (input.at(associated_list.at(j).point_id[k]).CPISeq - points.at(i).CPISeq);
                            elem_topush.SNR = input.at(associated_list.at(j).point_id[k]).SNR;
                            elem_topush.pitchAngle = input.at(associated_list.at(j).point_id[k]).pitch_angle;
                            elem_topush.azi_angle = input.at(associated_list.at(j).point_id[k]).azi_angle;
                            elem_topush.year = input.at(associated_list.at(j).point_id[k]).year;
                            elem_topush.month = input.at(associated_list.at(j).point_id[k]).month;
                            elem_topush.day = input.at(associated_list.at(j).point_id[k]).day;
                            elem_topush.hour = input.at(associated_list.at(j).point_id[k]).hour;
                            elem_topush.minute = input.at(associated_list.at(j).point_id[k]).minute;
                            elem_topush.second = input.at(associated_list.at(j).point_id[k]).second;
                            elem_topush.CPISeq = input.at(associated_list.at(j).point_id[k]).CPISeq;
                            elem_topush.biaozhi = input.at(associated_list.at(j).point_id[k]).biaozhi;
                        } else {
                            if (!m_id_cache.empty()) {
                                elem_topush.track_seq = m_id_cache.front();
                                m_id_cache.pop();
                            }
                            //time_interval = dt;
//                            time_interval =dt * (input.at(associated_list.at(j).point_id[k]).CPISeq - points.at(i).CPISeq);
                            elem_topush.SNR = input.at(associated_list.at(j).point_id[k]).SNR;
                            elem_topush.pitchAngle = input.at(associated_list.at(j).point_id[k]).pitch_angle;
                            elem_topush.azi_angle = input.at(associated_list.at(j).point_id[k]).azi_angle;
                            elem_topush.year = input.at(associated_list.at(j).point_id[k]).year;
                            elem_topush.month = input.at(associated_list.at(j).point_id[k]).month;
                            elem_topush.day = input.at(associated_list.at(j).point_id[k]).day;
                            elem_topush.hour = input.at(associated_list.at(j).point_id[k]).hour;
                            elem_topush.minute = input.at(associated_list.at(j).point_id[k]).minute;
                            elem_topush.second = input.at(associated_list.at(j).point_id[k]).second;
                            elem_topush.CPISeq = input.at(associated_list.at(j).point_id[k]).CPISeq;
                            elem_topush.biaozhi = input.at(associated_list.at(j).point_id[k]).biaozhi;
                        }
                        pos_x = input.at(associated_list.at(j).point_id[k]).pos_x;
                        pos_y = input.at(associated_list.at(j).point_id[k]).pos_y;
                        pos_z = input.at(associated_list.at(j).point_id[k]).pos_z;

                        //track type
                        elem_topush.track_state = DETECTIVE_TRACK;

                        //filtered
                        elem_topush.filtered[0] = pos_x;
                        elem_topush.filtered[1] = pos_y;
                        elem_topush.filtered[2] = pos_z;
                        elem_topush.filtered[3] = (pos_x - points.at(i).pos_x) / dt;
                        elem_topush.filtered[4] = (pos_y - points.at(i).pos_y) / dt;
                        elem_topush.filtered[5] = (pos_z - points.at(i).pos_z) / dt;

                        //predicted
                        memset(elem_topush.accumulation, 0x0, sizeof(float) * 3);
                        float prod_1[6 * 1], prod_2[6 * 1];
                        m_MatrixOprt->Mats_multiply(m_KalmanFilterParam.B, elem_topush.accumulation, prod_1, 6, 3, 1);
                        m_MatrixOprt->Mats_multiply(m_KalmanFilterParam.F, elem_topush.filtered, prod_2, 6, 6, 1);
                        m_MatrixOprt->MatAdd(prod_1, prod_2, elem_topush.predicted, 6, 1);


                        //doppler information
                        elem_topush.doppler_filtered[0] = (float) input.at(associated_list.at(j).point_id[k]).doppler;
                        elem_topush.doppler_filtered[1] =
                                (input.at(associated_list.at(j).point_id[k]).doppler - (float) points.at(i).doppler)
                                / dt;
                        m_MatrixOprt->Mats_multiply(m_KalmanFilterParam.F_v, elem_topush.doppler_filtered,
                                                    elem_topush.doppler_predicted, 2, 2, 1);

                        //error-matrix
                        memcpy(elem_topush.doppler_filtered_error_matrix, eye2,
                               sizeof(float) * DOPPLER_DEMENSION * DOPPLER_DEMENSION);
                        memcpy(elem_topush.filtered_error_matrix, eye6,
                               sizeof(float) * STATE_DEMENSION * STATE_DEMENSION);

                        //threshold
                        elem_topush.start_threshold = 1;
                        elem_topush.deletion_count = 0;
                        elem_topush.start_count = 0;            /*points.at(i).associate_count*/
                        elem_topush.deletion_threshold = 1;     //new 2
                        elem_topush.distance_increament = points.at(i).distance_increament;

                        //others
                        elem_topush.isUpdated = false;
                        elem_topush.isManeuverable = false;
                        //elem_topush.dotbianhao = 2;  //new  只在稳态航迹关联和补点中更新，暂态航迹不对其操作
                        elem_topush.updated_count = 1;
                        elem_topush.unupdated_count = 0;
                        elem_topush.range_0 = sqrt(powf(pos_x, 2) + powf(pos_y, 2) + powf(pos_z, 2));

                        if (input.at(associated_list.at(j).point_id[k]).velocity > 0)
                            elem_topush.velocity = sqrt(
                                    powf(elem_topush.predicted[2], 2.0f) + powf(elem_topush.predicted[3], 2.0f));
                        else
                            elem_topush.velocity = (-1.0f) * sqrt(powf(elem_topush.predicted[2], 2.0f) +
                                                                  powf(elem_topush.predicted[3], 2.0f));
                        track.push_back(elem_topush);
                    }
                    /*m_id_cache.push(points.at(i).point_seq);*/
                }
            }
        }
    }
}

void DataProcessing::resolute_ambiguity(std::vector<TrackInfo> &active_track,
                                        std::vector<associated_struct> &active_asslist,
                                        std::vector<TrackInfo> &detective_track,
                                        std::vector<associated_struct> &detective_asslist) {
    //稳态航迹分叉   起始一个新暂定航迹
    associated_struct elem;
    for (std::size_t i = 0; i < active_asslist.size(); i++) {
        if (active_asslist.at(i).associated_count > 1) {
            for (std::size_t j = 0; j < active_track.size(); j++) {
                if (active_track.at(j).track_seq == active_asslist.at(i).track_id) {
                    for (uint32_t k = 1; k < active_asslist.at(i).associated_count; k++) {
                        detective_track.push_back(active_track.at(j));
                        detective_track.at(detective_track.size() - 1).track_state = DETECTIVE_TRACK;
                        detective_track.at(detective_track.size() - 1).track_seq = m_id_cache.front();
                        detective_track.at(detective_track.size() - 1).deletion_threshold = 0;
                        detective_track.at(detective_track.size() - 1).start_count = 0;
                        detective_track.at(detective_track.size() - 1).isUpdated = true;
                        detective_track.at(detective_track.size() - 1).dotbianhao = 2;    //new 暂态航迹dotbianhao从2开始
                        m_id_cache.pop();
                        elem.track_id = detective_track.at(detective_track.size() - 1).track_seq;
                        elem.point_id[0] = active_asslist.at(i).point_id[k];
                        elem.associated_count = 1;

                        detective_asslist.push_back(elem);
                    }
                }
            }
        }
    }
}

void DataProcessing::append_new_points(std::vector<PointInfo> &input, std::vector<PointTemp> &output) {
    PointTemp elem_topush;
    for (std::size_t i = 0; i < input.size(); i++) {
        if (input.at(i).correState == NONE) {
            if (!m_id_cache.empty()) {
                input.at(i).point_seq = m_id_cache.front();
                m_id_cache.pop();
            }

            elem_topush.associate_count = 0;
            elem_topush.three_circle_jishu = 1;
            elem_topush.livetime = 1;
            elem_topush.state = DETECTIVE_POINT;
            elem_topush.isUpdated = false;
            elem_topush.distance_increament = fabs(input.at(i).velocity) * dt;
            /*output.push_back(input.at(i));*/

            elem_topush.point_seq = input.at(i).point_seq;
            elem_topush.pos_x = input.at(i).pos_x;
            elem_topush.pos_y = input.at(i).pos_y;
            elem_topush.pos_z = input.at(i).pos_z;
            elem_topush.vel_x = input.at(i).vel_x;
            elem_topush.vel_y = input.at(i).vel_y;
            elem_topush.doppler = input.at(i).doppler;
            elem_topush.velocity = input.at(i).velocity;
            elem_topush.amplitude = input.at(i).amplitude;
            elem_topush.SNR = input.at(i).SNR;
            elem_topush.CPISeq = input.at(i).CPISeq;
            elem_topush.pitch_angle = input.at(i).pitch_angle;
            elem_topush.azi_angle = input.at(i).azi_angle;
            elem_topush.noise = input.at(i).noise;
            elem_topush.year = input.at(i).year;
            elem_topush.month = input.at(i).month;
            elem_topush.day = input.at(i).day;
            elem_topush.hour = input.at(i).hour;
            elem_topush.minute = input.at(i).minute;
            elem_topush.second = input.at(i).second;
            elem_topush.biaozhi = input.at(i).biaozhi;
            elem_topush.r = input.at(i).r;

            output.push_back(elem_topush);
        }
    }
}

void DataProcessing::initialize() {

    for (std::size_t i = 0; i < m_detective_track_vec.size(); i++) {
        m_detective_track_vec.at(i).isUpdated = false;
        m_detective_track_vec.at(i).isManeuverable = false;
    }
    for (std::size_t i = 0; i < m_active_track_vec.size(); i++) {
        m_active_track_vec.at(i).isUpdated = false;
        m_active_track_vec.at(i).isManeuverable = false;
    }
    for (std::size_t i = 0; i < m_points_vec.size(); i++)
        m_points_vec.at(i).isUpdated = false;

    m_active_associated_list.clear();
    m_detective_associated_list.clear();
    m_points_associated_list.clear();
}


void DataProcessing::setFilterParams(filterChangedParams &filterChangedParams) {
    //Q
    float delta_w = filterChangedParams.param_Q;

    m_KalmanFilterParam.Q[0 * 6 + 0] = delta_w * 5.0625;//a
    m_KalmanFilterParam.Q[0 * 6 + 3] = delta_w * 3.3750;//a
    m_KalmanFilterParam.Q[1 * 6 + 1] = delta_w * 5.0625;//a
    m_KalmanFilterParam.Q[1 * 6 + 4] = delta_w * 3.3750;//a
    m_KalmanFilterParam.Q[2 * 6 + 2] = delta_w * 5.0625;//c
    m_KalmanFilterParam.Q[2 * 6 + 5] = delta_w * 3.3750;//c
    m_KalmanFilterParam.Q[3 * 6 + 0] = delta_w * 3.3750;//c
    m_KalmanFilterParam.Q[3 * 6 + 3] = delta_w * 2.25;//c
    m_KalmanFilterParam.Q[4 * 6 + 1] = delta_w * 3.3750;//c
    m_KalmanFilterParam.Q[4 * 6 + 4] = delta_w * 2.25;//c
    m_KalmanFilterParam.Q[5 * 6 + 2] = delta_w * 3.3750;//c
    m_KalmanFilterParam.Q[5 * 6 + 5] = delta_w * 2.25;//c

    m_KalmanFilterParam.R[0 * 3 + 0] = powf(0.3f * 3.57f, 2.0f);
    m_KalmanFilterParam.R[1 * 3 + 1] = powf(0.2f * 11.0f * pi / 180.0f, 2.0f);
    m_KalmanFilterParam.R[2 * 3 + 2] = powf(0.2f * 11.0f * pi / 180.0f, 2.0f);

    //R
    // m_KalmanFilterParam.R[0 * 2 + 0] = powf(filterChangedParams.Error_R, 2.0f);
    // m_KalmanFilterParam.R[1 * 2 + 1] = powf(filterChangedParams.Error_azimuth * pi / 180.0f, 2.0f);


    //inc
    m_a_inc_x = filterChangedParams.acc_x;
    m_a_inc_y = filterChangedParams.acc_y;
    m_a_inc_z = filterChangedParams.acc_z;

    //ref
    m_range_threshold_ref = filterChangedParams.range_ref;

    //azimuth
    //m_azimuth_threshold_ref = filterChangedParams.azimuth_ref;
    m_azimuth_threshold_ref = filterChangedParams.azimuth_ref * pi / 180;

    //接收的点迹
    active_id = filterChangedParams.active_id;
    dotnum_choose_flag = 0;
}






